#include "ParaBuilder.h"

using namespace Para;

#define SIGN(a) ((a) >= 0 ? 1 : -1)
#define ITERATION 10

Para::ParaBuilder::ParaBuilder(Handle(TColgp_HArray1OfPnt) cloud) : m_cloud(cloud)
{
}

Para::ParaBuilder::ParaBuilder(const TColgp_SequenceOfPnt& cloud) : m_cloud(new TColgp_HArray1OfPnt)
{
	m_cloud->Resize(1, cloud.Size(), false);
	for (int i = 1; i <= m_cloud->Size(); i++)
		m_cloud->SetValue(i, cloud.Value(i));
}

Handle(Geom_BSplineSurface) ParaBuilder::Build(std::vector<double>& u, std::vector<double>& v, int Nu, int Nv, int p, int q, std::vector<double>& w, double alpha, double beta, int times)
{
	Handle(TColgp_HArray1OfPnt) cloud = m_cloud;

	// 计算有向包围盒
	std::vector<gp_Vec> box = OBB(cloud);

	// 最长的两个方向，构造初始投影平面
	std::vector<gp_Vec> oxy(3);
	oxy[0] = box.back() + box[1] + box[3];
	oxy[1] = box[0].Normalized();
	oxy[2] = box[2].Normalized();

	// 计算初始投影参数
	int N = cloud->Size();
	for (int i = 0; i < N; i++)
	{
		gp_Vec a(cloud->Value(i + 1).XYZ());
		u[i] = oxy[1].Dot(a - oxy[0]);
		v[i] = oxy[2].Dot(a - oxy[0]);
	}

	// 标准化到 [0,1] 上
	Normalize(u);
	Normalize(v);

	// 计算误差
	Handle(Geom_BSplineSurface) surf;
	while (times > 0)
	{
		// 最小化弦长拟合曲面
		surf = Chord(cloud, u, v, Nu, Nv, p, q, w, alpha, beta);

		TIMER_START(PROJECTION);
		// 更新参数
		for (int i = 0; i < N; i++)
			Projection(surf, cloud->Value(i + 1), u[i], v[i], 1e-8);
		TIMER_END(PROJECTION);


		// 增加控制网格规模
		Nu *= 2, Nv *= 2;
		times--;
	}

	return surf;
}

std::vector<gp_Vec> ParaBuilder::OBB(Handle(TColgp_HArray1OfPnt) points)
{
	int n = points->Size();

	// 坐标分离为 3 个随机变量
	Eigen::VectorXd X(n), Y(n), Z(n);
	for (int i = 0; i < n; i++)
	{
		X[i] = points->Value(i + 1).X();
		Y[i] = points->Value(i + 1).Y();
		Z[i] = points->Value(i + 1).Z();
	}

	// 计算期望点
	double E[] = { 0, 0, 0 };
	E[0] = X.sum() / n;
	E[1] = Y.sum() / n;
	E[2] = Z.sum() / n;

	// 计算协方差矩阵
	Eigen::MatrixXd B(3, 3);
	B.setZero();
	for (int i = 0; i < n; i++)
	{
		B(0, 0) += X[i] * X[i];
		B(0, 1) += X[i] * Y[i];
		B(0, 2) += X[i] * Z[i];

		B(1, 0) += Y[i] * X[i];
		B(1, 1) += Y[i] * Y[i];
		B(1, 2) += Y[i] * Z[i];

		B(2, 0) += Z[i] * X[i];
		B(2, 1) += Z[i] * Y[i];
		B(2, 2) += Z[i] * Z[i];
	}
	B = B / n;

	// 计算 Xi_i 和 Xi_j 的协方差
	for (int i = 0; i < 3; i++)
		for (int j = 0; j < 3; j++)
			B(i, j) -= E[i] * E[j];

	// 特征分解
	Eigen::EigenSolver<Eigen::MatrixXd> es(B);

	// 获得特征值，取实部
	Eigen::VectorXcd cval = es.eigenvalues();
	Eigen::VectorXd val(cval.size());
	for (int i = 0; i < val.size(); i++)
		val[i] = cval[i].real();

	// 获得特征向量的矩阵，取实部
	Eigen::MatrixXcd cV = es.eigenvectors();
	Eigen::MatrixXd V(cV.rows(), cV.cols());
	for (int i = 0; i < V.rows(); i++)
		for (int j = 0; j < V.cols(); j++)
			V(i, j) = cV(i, j).real();

	// 比较对角元，进行排序
	int order[] = { 0, 1, 2 };
	for (int i = 0; i < 3; i++)
	{
		for (int j = 1; j < 3; j++)
		{
			// 比较相邻的元素，如果后者大于前者就交换
			if (val[j] > val[j - 1])
			{
				int tmp = order[j - 1];
				order[j - 1] = order[j];
				order[j] = tmp;

				double lambda = val[j - 1];
				val[j - 1] = val[j];
				val[j] = lambda;
			}
		}
	}

	// 三个主方向，按照从大到小的顺序
	gp_Vec x(V(0, order[0]), V(1, order[0]), V(2, order[0]));
	gp_Vec y(V(0, order[1]), V(1, order[1]), V(2, order[1]));
	gp_Vec z(V(0, order[2]), V(1, order[2]), V(2, order[2]));
	x.Normalize();
	y.Normalize();
	z.Normalize();

	// 计算在主方向上的投影长度，需要将期望平移到原点
	gp_Vec Ev(E[0], E[1], E[2]);
	Eigen::MatrixXd M(3, 2);
	M.setZero();
	for (int i = 0; i < n; i++)
	{
		gp_Vec v = gp_Vec(X[i], Y[i], Z[i]) - Ev;

		double p[] = { v.Dot(x), v.Dot(y), v.Dot(z) };
		for (int j = 0; j < 3; j++)
		{
			if (M(j, 0) > p[j])
				M(j, 0) = p[j];
			if (M(j, 1) < p[j])
				M(j, 1) = p[j];
		}
	}

	// 记录在 x,y,z 正反方向的 6 个向量，以及中心向量 Ev
	std::vector<gp_Vec> box;
	for (int j = 0; j < 2; j++)
		box.push_back(x * M(0, j));
	for (int j = 0; j < 2; j++)
		box.push_back(y * M(1, j));
	for (int j = 0; j < 2; j++)
		box.push_back(z * M(2, j));
	box.push_back(Ev);

	// 调整法向方向
	if (box[4].Dot({ 0, 1, 0 }) < 0)
		for (int i = 0; i < 5; i++)
			box[i] = box[i] * -1;

	// 调整为右手坐标系，两个方向的先后顺序会决定光照时的法向
	if (box[4].Dot(box[0].Crossed(box[2])) < 0)
	{
		gp_Vec tx = box[0];
		box[0] = box[2];
		box[2] = tx;

		// 注意正反方向的都要交换
		tx = box[1];
		box[1] = box[3];
		box[3] = tx;
	}

	return box;
}

void ParaBuilder::Normalize(std::vector<double>& u)
{
	int m = u.size() - 1;
	double umin = u[0];
	double umax = u[0];
	for (int i = 0; i < m + 1; i++)
	{
		if (umin > u[i])
			umin = u[i];
		if (umax < u[i])
			umax = u[i];
	}

	double du = umax - umin;
	for (int i = 0; i < m + 1; i++)
		u[i] = (u[i] - umin) / du;
}

int ParaBuilder::FindSpan(double u, int p, const std::vector<double>& knots)
{
	// 防止越界
	assert(u >= knots[p]);
	assert(u <= knots[knots.size() - p - 1]);

	// 返回左闭右开区间 u \in [u_i, u_{i+1})
	int m = knots.size() - 1;
	int n = m - p - 1;

	// 边界情况：如果 u 在区间最大值的边界上，就返回左边的区间
	// 注意这里边界是最大的值，而不是节点最大值
	if (u == knots[knots.size() - p - 1])
		return n;

	// 二分法寻找区间
	int low = p;
	int high = n + 1;
	int mid = (low + high) / 2;
	while (u < knots[mid] || u >= knots[mid + 1])
	{
		if (u < knots[mid])
			high = mid;
		else
			low = mid;
		mid = (low + high) / 2;
	}
	return mid;
}

std::vector<double> ParaBuilder::BasisFuncs(int i, double u, int p, const std::vector<double>& knots)
{
	// 防止越界，确保索引在有效区间内
	assert(i >= p);
	assert(i < int(knots.size() - p - 1));
	assert(u >= knots[p]);
	assert(u <= knots[knots.size() - p - 1]);

	std::vector<double> N(p + 1);     // 存放 u \in [u_i, u_{i+1}) 处不为零的基函数值
	std::vector<double> left(p + 1);  // left[j] = u - u_{i+1-j}
	std::vector<double> right(p + 1); // right[j] = u_{i+j} - u

	N[0] = 1;
	for (int j = 1; j < p + 1; j++)
	{
		left[j] = u - knots[i + 1 - j];
		right[j] = knots[i + j] - u;

		double saved = 0;
		for (int r = 0; r < j; r++)
		{
			double tmp = N[r] / (right[r + 1] + left[j - r]);
			N[r] = saved + right[r + 1] * tmp;
			saved = left[j - r] * tmp;
		}
		N[j] = saved;
	}
	return N;
}

void Para::ParaBuilder::Projection(Handle(Geom_BSplineSurface) surf, gp_Pnt P, double& u, double& v, double tol)
{
	// 曲率投影
	int times = 0;
	double angle = tol * 10;
	Eigen::VectorXd uv(2);
	uv[0] = u;
	uv[1] = v;

	while (angle > tol && times < ITERATION)
	{
		// 计算各阶导向量
		gp_Pnt S00;
		gp_Vec S10, S01, S20, S02, S11;
		surf->D2(uv[0], uv[1], S00, S10, S01, S20, S02, S11);

		// 投影方向
		gp_Vec dP = gp_Vec(P.XYZ()) - gp_Vec(S00.XYZ());

		// 获得 I,II 基本形式的矩阵
		gp_Vec N = S10.Crossed(S01).Normalized();

		Eigen::MatrixXd I(2, 2);
		I(0, 0) = S10.Dot(S10);
		I(0, 1) = I(1, 0) = S10.Dot(S01);
		I(1, 1) = S01.Dot(S01);

		Eigen::MatrixXd II(2, 2);
		II(0, 0) = N.Dot(S20);
		II(0, 1) = II(1, 0) = N.Dot(S11);
		II(1, 1) = N.Dot(S02);

		// 沿着 Su Sv 方向进行分解（利用线性方程组）
		Eigen::VectorXd b(2);
		b[0] = dP.Dot(S10);
		b[1] = dP.Dot(S01);

		// Cholesky 分解进行求解
		Eigen::LLT<Eigen::MatrixXd> llt(I);
		Eigen::VectorXd lambda = llt.solve(b);

		// 计算法曲率
		double h = 0;
		for (int i = 0; i < 2; i++)
			for (int j = 0; j < 2; j++)
				h += II(i, j) * lambda[i] * lambda[j];

		double g = 0;
		for (int i = 0; i < 2; i++)
			for (int j = 0; j < 2; j++)
				g += I(i, j) * lambda[i] * lambda[j];

		// 法曲率，法曲率圆心，投影点 Q
		double kappa = h / g;
		double R = fabs(1 / kappa);
		gp_Vec O = gp_Vec(S00.XYZ()) + N / kappa;
		gp_Vec Q = O + (gp_Vec(P.XYZ()) - O).Normalized() * R;

		// 计算增量
		double dt = SIGN((Q - gp_Vec(S00.XYZ())).Dot(S10 * lambda[0] + S01 * lambda[1]));
		uv[0] += dt * lambda[0];
		uv[1] += dt * lambda[1];

		// 防止越界
		if (uv[0] <= 0)
			uv[0] = tol;
		if (uv[0] >= 1)
			uv[0] = 1 - tol;
		if (uv[1] <= 0)
			uv[1] = tol;
		if (uv[1] >= 1)
			uv[1] = 1 - tol;

		// 计算夹角
		angle = N.Crossed(dP).Magnitude() / dP.Magnitude();
		times++;
	}

	// 更新参数
	u = uv[0];
	v = uv[1];
}

void Para::ParaBuilder::OCCProjection(Handle(Geom_BSplineSurface) surf, gp_Pnt P, double& u, double& v)
{
	GeomAPI_ProjectPointOnSurf Proj(P, surf, 0, 1, 0, 1);
	Proj.LowerDistanceParameters(u, v);
}

Handle(Geom_BSplineSurface) Para::ParaBuilder::Chord(Handle(TColgp_HArray1OfPnt) cloud, std::vector<double>& u, std::vector<double>& v, int Nu, int Nv, int p, int q, std::vector<double>& w, double alpha, double beta)
{
	// N 个控制点
	int nu = Nu - 1;
	int nv = Nv - 1;
	int m = cloud->Size() - 1;

	// 节点向量
	std::vector<double> uKnots(Nu + p + 1);
	std::vector<double> vKnots(Nv + q + 1);

	// u 节点
	for (int i = 0; i < p + 1; i++)
		uKnots[i] = 0;
	for (int i = p + 1; i < Nu; i++)
		uKnots[i] = 1.0 * (i - p) / (Nu - p);
	for (int i = Nu; i < Nu + p + 1; i++)
		uKnots[i] = 1;

	// v 节点
	for (int i = 0; i < q + 1; i++)
		vKnots[i] = 0;
	for (int i = q + 1; i < Nv; i++)
		vKnots[i] = 1.0 * (i - q) / (Nv - q);
	for (int i = Nv; i < Nv + q + 1; i++)
		vKnots[i] = 1;

	// 填充最小二乘系数矩阵
	std::vector<Eigen::Triplet<double>> tripA;
	for (int k = 0; k < m + 1; k++)
	{
		int uSpan = FindSpan(u[k], p, uKnots);
		int vSpan = FindSpan(v[k], q, vKnots);

		std::vector<double> Bu = BasisFuncs(uSpan, u[k], p, uKnots);
		std::vector<double> Bv = BasisFuncs(vSpan, v[k], q, vKnots);

		// 每次填充 (p + 1) * (q + 1) 规模的元素
		int uIndex = (uSpan - p) * (nv + 1);
		for (int i = 0; i < p + 1; i++)
		{
			int vIndex = vSpan - q;
			for (int j = 0; j < q + 1; j++)
				tripA.push_back(Eigen::Triplet<double>(k, uIndex + vIndex + j, Bu[i] * Bv[j]));

			uIndex += nv + 1;
		}
	}

	Eigen::SparseMatrix<double> A(m + 1, (nu + 1) * (nv + 1));
	A.setFromTriplets(tripA.begin(), tripA.end());

	// 构造系数矩阵，加上最小化弦长的权重，增加最小化切向变化的约束
	int Nuv = (nu + 1) * (nv + 1);
	double gamma = 1 - alpha - beta;

	// 先添加对角块
	std::vector<Eigen::Triplet<double>> tripB;
	for (int i = 0; i < nu + 1; i++)
	{
		int ii = i * (nv + 1);

		// 最小化弦长的边界
		tripB.push_back(Eigen::Triplet<double>(ii, ii, alpha));
		tripB.push_back(Eigen::Triplet<double>(ii, ii + 1, -alpha));

		tripB.push_back(Eigen::Triplet<double>(ii + nv, ii + nv, alpha));
		tripB.push_back(Eigen::Triplet<double>(ii + nv, ii + nv - 1, -alpha));

		// 最小化导数的边界
		tripB.push_back(Eigen::Triplet<double>(ii, ii, beta));
		tripB.push_back(Eigen::Triplet<double>(ii, ii + 1, -2 * beta));
		tripB.push_back(Eigen::Triplet<double>(ii, ii + 2, beta));

		tripB.push_back(Eigen::Triplet<double>(ii + 1, ii, -2 * beta));
		tripB.push_back(Eigen::Triplet<double>(ii + 1, ii + 1, 5 * beta));
		tripB.push_back(Eigen::Triplet<double>(ii + 1, ii + 2, -4 * beta));
		tripB.push_back(Eigen::Triplet<double>(ii + 1, ii + 3, beta));

		tripB.push_back(Eigen::Triplet<double>(ii + nv, ii + nv, beta));
		tripB.push_back(Eigen::Triplet<double>(ii + nv, ii + nv - 1, -2 * beta));
		tripB.push_back(Eigen::Triplet<double>(ii + nv, ii + nv - 2, beta));

		tripB.push_back(Eigen::Triplet<double>(ii + nv - 1, ii + nv, -2 * beta));
		tripB.push_back(Eigen::Triplet<double>(ii + nv - 1, ii + nv - 1, 5 * beta));
		tripB.push_back(Eigen::Triplet<double>(ii + nv - 1, ii + nv - 2, -4 * beta));
		tripB.push_back(Eigen::Triplet<double>(ii + nv - 1, ii + nv - 3, beta));

		// 最小化弦长的中间部分
		for (int j = 1; j < nv; j++)
		{
			tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j, 2 * alpha));
			tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j - 1, -alpha));
			tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j + 1, -alpha));
		}

		// 最小化导数的中间部分
		for (int j = 2; j < nv - 1; j++)
		{
			tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j, 6 * beta));
			tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j - 1, -4 * beta));
			tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j + 1, -4 * beta));
			tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j - 2, beta));
			tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j + 2, beta));
		}

		// 添加最小化弦长的中间部分
		if (i > 0 && i < nu)
		{
			for (int j = 0; j < nv + 1; j++)
			{
				tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j, 2 * alpha));
				tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j + nv + 1, -alpha));
				tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j - (nv + 1), -alpha));
			}
		}

		// 添加最小化导数的中间部分
		if (i > 1 && i < nu - 1)
		{
			for (int j = 0; j < nv + 1; j++)
			{
				tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j, 6 * beta));
				tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j + nv + 1, -4 * beta));
				tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j - (nv + 1), -4 * beta));
				tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j + (nv + 1) * 2, beta));
				tripB.push_back(Eigen::Triplet<double>(ii + j, ii + j - (nv + 1) * 2, beta));
			}
		}
	}

	// 处理前两行与最后两行
	for (int i = 0; i < nv + 1; i++)
	{
		// 第一行
		tripB.push_back(Eigen::Triplet<double>(i, i, alpha + beta));
		tripB.push_back(Eigen::Triplet<double>(i, i + nv + 1, -alpha - 2 * beta));
		tripB.push_back(Eigen::Triplet<double>(i, i + (nv + 1) * 2, beta));

		// 第二行
		tripB.push_back(Eigen::Triplet<double>(i + nv + 1, i, -2 * beta));
		tripB.push_back(Eigen::Triplet<double>(i + nv + 1, i + nv + 1, 5 * beta));
		tripB.push_back(Eigen::Triplet<double>(i + nv + 1, i + (nv + 1) * 2, -4 * beta));
		tripB.push_back(Eigen::Triplet<double>(i + nv + 1, i + (nv + 1) * 3, beta));

		// 最后一行
		int index = nu * (nv + 1) + i;
		tripB.push_back(Eigen::Triplet<double>(index, index, alpha + beta));
		tripB.push_back(Eigen::Triplet<double>(index, index - (nv + 1), -alpha - 2 * beta));
		tripB.push_back(Eigen::Triplet<double>(index, index - (nv + 1) * 2, beta));

		// 倒数第二行
		tripB.push_back(Eigen::Triplet<double>(index - nv - 1, index, -2 * beta));
		tripB.push_back(Eigen::Triplet<double>(index - nv - 1, index - nv - 1, 5 * beta));
		tripB.push_back(Eigen::Triplet<double>(index - nv - 1, index - (nv + 1) * 2, -4 * beta));
		tripB.push_back(Eigen::Triplet<double>(index - nv - 1, index - (nv + 1) * 3, beta));
	}

	Eigen::SparseMatrix<double> B((nu + 1) * (nv + 1), (nu + 1) * (nv + 1));
	B.setFromTriplets(tripB.begin(), tripB.end());

	Eigen::SparseMatrix<double> C = B + A.transpose() * A * gamma;

	// Cholesky 分解
	Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> ldlt(C);

	// 构造最小二乘+能量约束系统的右端矩阵
	Eigen::MatrixXd cv(m + 1, 3);
	for (int j = 0; j < m + 1; j++)
	{
		cv(j, 0) = cloud->Value(j + 1).X();
		cv(j, 1) = cloud->Value(j + 1).Y();
		cv(j, 2) = cloud->Value(j + 1).Z();
	}
	Eigen::MatrixXd b = A.transpose() * cv * gamma;

	// 求解线性系统，并获得控制点
	TColgp_Array2OfPnt cpts(1, nu + 1, 1, nv + 1);
	Eigen::MatrixXd X = ldlt.solve(b);
	for (int i = 0; i < nu + 1; i++)
	{
		for (int j = 0; j < nv + 1; j++)
		{
			int index = i * (nv + 1) + j;
			cpts.SetValue(i + 1, j + 1, { X(index, 0), X(index, 1), X(index, 2) });
		}
	}

	// 生成 OCC 的样条曲面
	TColStd_Array1OfReal UKnots(1, Nu - p + 1);
	TColStd_Array1OfReal VKnots(1, Nv - q + 1);
	TColStd_Array1OfInteger UMults(1, Nu - p + 1);
	TColStd_Array1OfInteger VMults(1, Nv - q + 1);

	for (int i = 0; i < Nu - p + 1; i++)
	{
		UKnots.SetValue(i + 1, uKnots[i + p]);
		UMults.SetValue(i + 1, 1);
	}

	for (int i = 0; i < Nv - q + 1; i++)
	{
		VKnots.SetValue(i + 1, vKnots[i + q]);
		VMults.SetValue(i + 1, 1);
	}

	// 两端重数
	UMults.SetValue(1, p + 1);
	UMults.SetValue(Nu - p + 1, p + 1);
	VMults.SetValue(1, q + 1);
	VMults.SetValue(Nv - q + 1, q + 1);

	return new Geom_BSplineSurface(cpts, UKnots, VKnots, UMults, VMults, p, q);
}